#include "../../include/PX4_realsense_bridge/PX4_realsense_bridge.h"

#include <atomic>
#include <condition_variable>
#include <mutex>
#include <string>
#include <thread>
#include <unionsys_core.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#define PI 3.14159265358979323846
/***************************param************************************/
double rot_pitch = 0;//输入旋转的角度，例如90度
double rot_roll = 180;
double rot_yaw = 0;
double roll, pitch, yaw;
tf::StampedTransform ts;//用来发布无人机当前位置的坐标系坐标轴
tf::TransformBroadcaster *tfBroadcasterPointer;	//广播坐标轴
namespace bridge {

PX4_Realsense_Bridge::PX4_Realsense_Bridge(const ros::NodeHandle& nh)
    : nh_(nh) {

  // initialize subscribers
  tf::TransformBroadcaster tfBroadcaster;
  tfBroadcasterPointer = &tfBroadcaster;
  // odom_sub_ = nh_.subscribe<const nav_msgs::Odometry&>("/odometry/assign_to_gps", 10, &PX4_Realsense_Bridge::odomCallback, this);
  odom_sub_ = nh_.subscribe<const nav_msgs::Odometry&>("/Odometry", 10, &PX4_Realsense_Bridge::odomCallback, this);

  // odom_sub_ = nh_.subscribe<const nav_msgs::Odometry&>("/camera/odom/sample_throttled", 10, &PX4_Realsense_Bridge::odomCallback, this);
  // publishers
  mavros_odom_pub_ =
      nh_.advertise<nav_msgs::Odometry>("/mavros/odometry/out", 10);
  mavros_system_status_pub_ =
      nh_.advertise<mavros_msgs::CompanionProcessStatus>("/mavros/companion_process/status", 1);

  last_callback_time = ros::Time::now();

  status_mutex_.reset(new std::mutex);
  worker_ = std::thread(&PX4_Realsense_Bridge::publishSystemStatus, this);


};

PX4_Realsense_Bridge::~PX4_Realsense_Bridge() { }


void PX4_Realsense_Bridge::odomCallback(const nav_msgs::Odometry& msg) {

  // publish odometry msg
  nav_msgs::Odometry external_odom = msg;
  nav_msgs::Odometry output;
  tf2::Quaternion quat, rot;
  tf2::convert(msg.pose.pose.orientation, quat); //把mavros/local_position/pose里的四元数转给tf2::Quaternion quat
  tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
  rot = {rot_pitch*PI/180, rot_roll*PI/180, rot_yaw*PI/180};//pitch roll yaw
  quat*=rot;
  output.header.stamp = ros::Time::now();
  output.header.frame_id = "odom";
  output.child_frame_id = "base_link";
  output.pose.pose.position.x = external_odom.pose.pose.position.x;
  output.pose.pose.position.y = external_odom.pose.pose.position.y;
  output.pose.pose.position.z = external_odom.pose.pose.position.z;
  output.pose.pose.orientation.x = quat.getX();
  output.pose.pose.orientation.y = quat.getY();
  output.pose.pose.orientation.z = quat.getZ();
  output.pose.pose.orientation.w = quat.getW();
  mavros_odom_pub_.publish(output); 








  flag_first_pose_received = true;


  { // lock mutex
    std::lock_guard<std::mutex> status_guard(*(status_mutex_));

    last_system_status_ = system_status_;

    // check confidence in vision estimate by looking at covariance
    if( msg.pose.covariance[0] > 0.1 ) // low confidence -> reboot companion
    {
      system_status_ = MAV_STATE::MAV_STATE_FLIGHT_TERMINATION;
    }
    else if( msg.pose.covariance[0] >= 0.1 ) // medium confidence
    {
      system_status_ = MAV_STATE::MAV_STATE_CRITICAL;
    }
    else if( msg.pose.covariance[0] < 0.1 ) // high confidence
    {
      system_status_ = MAV_STATE::MAV_STATE_ACTIVE;
    }
    else
    {
      ROS_WARN_STREAM("Unexpected vision sensor variance");
    }  

    // publish system status immediately if it changed
    if( last_system_status_ != system_status_ )
    {
      mavros_msgs::CompanionProcessStatus status_msg;

      status_msg.header.stamp = ros::Time::now();
      status_msg.component = 197;  // MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY

      status_msg.state = (int)system_status_;

      mavros_system_status_pub_.publish(status_msg);
    }  


  last_callback_time = ros::Time::now();
    
  }


}


void PX4_Realsense_Bridge::publishSystemStatus(){
  

  while(ros::ok()){
    
    ros::Duration(1).sleep();

    if(flag_first_pose_received == true) { // only send heartbeat if we receive pose estimates at all

      // check if we received an recent update
      // otherwise let the companion computer restart
      if( (ros::Time::now()-last_callback_time) > ros::Duration(0.5) ){
        ROS_WARN_STREAM("Stopped receiving data from Vision");
        system_status_ = MAV_STATE::MAV_STATE_FLIGHT_TERMINATION;
      }

      mavros_msgs::CompanionProcessStatus status_msg;

      status_msg.header.stamp = ros::Time::now();
      status_msg.component = 197;  // MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY
    
      { // lock mutex
        std::lock_guard<std::mutex> status_guard(*(status_mutex_));

        status_msg.state = (int)system_status_;

        mavros_system_status_pub_.publish(status_msg);
      }
    }
  }

}

}
